/*
 * BaseStationController.seepeepee
 *
 *  Created on: Feb 5, 2010
 *      Author: fmason
 */

#include "components/RemoteControlRadio.h"
#include "arduinomega/HardwareSerial.h"
#include "components/Joystick.h"
#include "constants/command.h"

extern "C" void __cxa_pure_virtual(){};

int const DRIVE = 1;
Joystick joystick;

int main()
{
	int state = DRIVE;
	int left_power, right_power = OFF_DUTY;
	int left_direction, right_direction = -1;
	int servo_position = -1;

	init();
	Serial.begin( BAUD_RATE );
	Serial.println( "Setting up Radio...");
	setupRadioTransmitter();
	Serial.println( "Radio setup" );
	Serial.println( "Setting up joystick" );
	joystick.setup();
	Serial.println( "joystick setup" );

	while( true )
	{

		switch( state )
		{
			case DRIVE:
				joystick.printMotorInput();
				//Serial.println( "case: DRIVE" );

				//delay(2000);

				// get power and direction variable from joystick
				left_power = joystick.getLeftPower();
				right_power = joystick.getRightPower();
				left_direction = joystick.getLeftDirection();
				right_direction = joystick.getRightDirection();
				servo_position = joystick.getServoPosition();


				// set duty cycle state of Left motor
				if( left_power == FULL_DUTY )
				{
					Serial.println( "LEFT FULL" );
					sendCommand( SET_LEFT_MOTOR_TO_FULL_DUTY );
				}
				else if( left_power == HALF_DUTY )
				{
					Serial.println( "LEFT HALF" );
					sendCommand( SET_LEFT_MOTOR_TO_HALF_DUTY );
				}
				else
				{
					Serial.println( "LEFT OFF" );
					sendCommand( SET_LEFT_MOTOR_TO_OFF_DUTY );
				}

				if( left_direction == FORWARD )
				{
					sendCommand( SET_LEFT_MOTOR_FORWARD );
					Serial.println( "LEFT FWD" );
				}
				else
				{
					sendCommand( SET_LEFT_MOTOR_BACKWARD );
					Serial.println( "LEFT BKW" );
				}

				// set duty cycle state of Left motor
				if( right_power == FULL_DUTY )
				{
					Serial.println( "RIGHT FULL" );
					sendCommand( SET_RIGHT_MOTOR_TO_FULL_DUTY );
				}
				else if( right_power == HALF_DUTY )
				{
					Serial.println( "RIGHT HALF" );
					sendCommand( SET_RIGHT_MOTOR_TO_HALF_DUTY );
				}
				else
				{
					Serial.println( "RIGHT OFF" );
					sendCommand( SET_RIGHT_MOTOR_TO_OFF_DUTY );
				}

				if( right_direction == FORWARD )
				{
					sendCommand( SET_RIGHT_MOTOR_FORWARD );
					Serial.println( "RIGHT FWD" );
				}
				else
				{
					sendCommand( SET_RIGHT_MOTOR_BACKWARD );
					Serial.println( "RIGHT BKW" );
				}

				if( servo_position == RIGHT )
				{
					sendCommand( MOVE_SERVO_TO_RIGHT);
				}
				else if( servo_position == LEFT )
				{
					sendCommand( MOVE_SERVO_TO_LEFT );
				}
				else if( servo_position == MIDDLE )
				{
					sendCommand( MOVE_SERVO_TO_MIDDLE );
				}

				delay( SERVO_DELAY );
				break;
			default:
				break;
		}
	}
	return 1;
}
